#include <joyos.h>
#include <math.h>
#include "define.h"
#include "roboturn.h"
#include "robomove.h"


/*
 * "turn" turns the robot to the specified angle.
 * Angles are positive when turned counter-clockwise and negative when turned clockwise.
 * The angle is 0 when the direction is right in front of the robot.
 * 
 * @requires none
 * @effects the robot is turned to the specified angle.
 * @modifies none
 */
//void turn(uint16_t deg){
//	int (*turnThreadpt)(uint16_t deg) = turnThread;
//	create_thread(turnThreadpt, 1000, 0, "TurnThread");
//	pause(1000);
//}

//Shaft 150 per One Round Wheel							A
//12*M_PI*degree/360 >> (1/4) in. to turn 90 degree 	B // 12 have to check the width of the robot
//One Wheel = 3/8*9*M_PI in.							C
//Motor velocity 75 per 1 round wheel					D
//Formula = (B/C * A) = appropriate shaft num added both right and left

int shaftToAngle(int16_t shaft){
	double completeTurnDist = ROBOT_WIDTH * M_PI;
	double wheelTurned = (double)shaft / (double)SHAFT_PER_WHEEL;
	double circumTurned = DIST_PER_WHEEL * wheelTurned;
	
	return (int)((circumTurned / completeTurnDist) * 360);
}

int angleToShaft(int16_t degree){
	double completeTurnDist = ROBOT_WIDTH * M_PI;
	double circumTurned = ((double)degree / 360) * completeTurnDist; // the circumference dist turned by robot
	double wheelTurned = circumTurned / DIST_PER_WHEEL;
	int sign;
	(degree > 0) ? (sign = 1) : (sign = -1);
	
	return (int)(sign * wheelTurned * SHAFT_PER_WHEEL);
}

//Test Run to be integrated with gyro turn
void turnWithShaft(int16_t degree){
	bool turn_left;
	encoder_reset(ENCODER_PORT_LEFT);
	encoder_reset(ENCODER_PORT_RIGHT);
	gyro_set_degrees(0.0);
	
	int shaftToGet = angleToShaft(degree);
	int16_t default_turn_vel_left = 150;
	int16_t default_turn_vel_right = 200;
	int16_t turn_vel_left;
	int16_t turn_vel_right;
	
	if (degree >= 0)
		turn_left = true;
	else
		turn_left = false;
		
	int16_t beforeShaftLeft = encoder_read(ENCODER_PORT_LEFT);
	int16_t beforeShaftRight = encoder_read(ENCODER_PORT_RIGHT);
	int16_t currentShaft = 0;
	int16_t currentShaftLeft = 0;
	int16_t currentShaftRight = 0;
	
	status = 2;	
	shaftValueMonitorL = encoder_read(ENCODER_PORT_LEFT);
	shaftValueMonitorR = encoder_read(ENCODER_PORT_RIGHT);
	
	if (turn_left){
		turn_vel_left = -default_turn_vel_left;
		turn_vel_right = default_turn_vel_left;
		motor_set_vel(MOTOR_PORT_RIGHT, turn_vel_right);
		motor_set_vel(MOTOR_PORT_LEFT, turn_vel_left);
		int16_t startAngle = getCurrentAngle();
		
		while(currentShaft <=  shaftToGet){
			printf_P(PSTR("\nangle: %d L_vel: %d, R_vel: %d"), shaftToAngle(currentShaft),
							turn_vel_left, turn_vel_right);
			currentShaftLeft = encoder_read(ENCODER_PORT_LEFT) - beforeShaftLeft;
			currentShaftRight = encoder_read(ENCODER_PORT_RIGHT) - beforeShaftRight;
			
			turn_vel_left = turn_vel_left + (0* turnEncoderErr() + 1 * turnGyroErr(startAngle, ENCODER_PORT_LEFT));
			turn_vel_right = turn_vel_right - (0 * turnEncoderErr() + 1 * turnGyroErr(startAngle, ENCODER_PORT_RIGHT));
			currentShaft = (currentShaftLeft+currentShaftRight) / 2;
			pause(25);
		}
	}
	else{
		turn_vel_left = default_turn_vel_right;
		turn_vel_right = -default_turn_vel_right;
		motor_set_vel(MOTOR_PORT_RIGHT, turn_vel_right);
		motor_set_vel(MOTOR_PORT_LEFT, turn_vel_left);
		int16_t startAngle = getCurrentAngle();

		while(currentShaft <= shaftToGet){
			printf_P(PSTR("\nangle: %d L_vel: %d, R_vel: %d"), shaftToAngle(currentShaft),
							turn_vel_left, turn_vel_right);
			currentShaftLeft = encoder_read(ENCODER_PORT_LEFT) - beforeShaftLeft;
			currentShaftRight = encoder_read(ENCODER_PORT_RIGHT) - beforeShaftRight;
			
			turn_vel_left = turn_vel_left - (0 * turnEncoderErr() + 1 * turnGyroErr(startAngle, ENCODER_PORT_LEFT));
			turn_vel_right = turn_vel_right + (0 * turnEncoderErr() + 1 * turnGyroErr(startAngle, ENCODER_PORT_RIGHT));
			currentShaft = (currentShaftLeft+currentShaftRight) / 2;
			pause(25);
		}
	}
	roboStop();
}

double turnEncoderErr(){
	int16_t leftShaftTurn = encoder_read(ENCODER_PORT_LEFT);
	int16_t rightShaftTurn = encoder_read(ENCODER_PORT_RIGHT);
	return leftShaftTurn - rightShaftTurn;
}

double turnGyroErr(int16_t startAngle, uint8_t encoderPort){
	int16_t turnedAngle = getCurrentAngle() - startAngle;
	int16_t expectedShaft = angleToShaft(turnedAngle);
	
	int16_t turnedShaft = encoder_read(encoderPort);
	
	return expectedShaft - turnedShaft;
}
/*
void turnWithShaft(int16_t degree){
	bool turn_left;
	encoder_reset(ENCODER_PORT_LEFT);
	encoder_reset(ENCODER_PORT_RIGHT);
	
	int shaftToGet = angleToShaft(degree);
	int8_t time = 2; // 2 s    Assume motor max 255 turn per sec
	int16_t turn_motor_vel = (int16_t)(12*M_PI*(double)degree/360)/(3/8*9*M_PI)/time; // (B/C)/Time
	
	roboStop();
	
	if (degree >= 0)
		turn_left = true;
	else
		turn_left = false;
	
	int16_t beforeShaftLeft = encoder_read(ENCODER_PORT_LEFT);
	int16_t beforeShaftRight = encoder_read(ENCODER_PORT_RIGHT);
	int16_t currentShaft = 0;
	int16_t currentShaftLeft = 0;
	int16_t currentShaftRight = 0;		
	int start_time = get_time();
	
	status = 2;
	shaftValueMonitorL = encoder_read(ENCODER_PORT_LEFT);
	shaftValueMonitorR = encoder_read(ENCODER_PORT_RIGHT);			
	
	if (turn_left){
		while(currentShaft <=  shaftToGet && (get_time()-start_time) < TURN_TIME_LIMIT){
			printf_P(PSTR("\nangle: %d R_velocity: %d"),  shaftToAngle(currentShaft), turn_motor_vel);
			motor_set_vel(MOTOR_PORT_RIGHT, turn_motor_vel);
			motor_set_vel(MOTOR_PORT_LEFT, -turn_motor_vel);
			currentShaftLeft = encoder_read(ENCODER_PORT_LEFT) - beforeShaftLeft;
			currentShaftRight = encoder_read(ENCODER_PORT_LEFT) - beforeShaftRight;
			currentShaft = currentShaftLeft+currentShaftRight;
			pause(50);
		}
	}
	else{
		while(currentShaft >= shaftToGet && (get_time()-start_time) < TURN_TIME_LIMIT){
			printf_P(PSTR("\nangle: %d R_velocity: %d"), shaftToAngle(currentShaft), turn_motor_vel);
			motor_set_vel(MOTOR_PORT_RIGHT, -turn_motor_vel);
			motor_set_vel(MOTOR_PORT_LEFT, turn_motor_vel);
			currentShaftLeft = encoder_read(ENCODER_PORT_LEFT) - beforeShaftLeft;
			currentShaftRight = encoder_read(ENCODER_PORT_LEFT) - beforeShaftRight;
			currentShaft = currentShaftLeft+currentShaftRight;
			pause(50);
		}
	}
	roboStop();
}
*/
/*
void turn(int16_t degree){
	bool turn_left;
	int16_t turn_motor_vel = 0;
	
	roboStop();
	
	if (degree >= 0)
		turn_left = true;
	else
		turn_left = false;
	int16_t beforeAngle = getCurrentAngle();
	int16_t currentAngle = 0;
	int start_time = get_time();
	status = 2;
	shaftValueMonitorL = encoder_read(ENCODER_PORT_LEFT);
	shaftValueMonitorR = encoder_read(ENCODER_PORT_RIGHT);
	if (turn_left){
		while(currentAngle <= degree && (get_time()-start_time) < TURN_TIME_LIMIT){
			//printf_P(PSTR("\nangle: %d"), currentAngle);
			turn_motor_vel = (int)((fabs(degree - currentAngle) / degree) * MAX_TURN_MOTOR_VEL);
			motor_set_vel(MOTOR_PORT_RIGHT, turn_motor_vel);
			motor_set_vel(MOTOR_PORT_LEFT, -turn_motor_vel);
			currentAngle = getCurrentAngle() - beforeAngle;
			pause(50);
		}
	}
	else{
		while(currentAngle >= degree && (get_time()-start_time) < TURN_TIME_LIMIT){
			//printf_P(PSTR("\nangle: %d R_velocity: %d"), currentAngle, turn_motor_vel);
			turn_motor_vel = (int16_t)((fabs(degree - currentAngle) / degree) * MAX_TURN_MOTOR_VEL);
			motor_set_vel(MOTOR_PORT_RIGHT, -turn_motor_vel);
			motor_set_vel(MOTOR_PORT_LEFT, turn_motor_vel);
			currentAngle = getCurrentAngle() - beforeAngle;
			pause(50);
		}
	}
	roboStop();
}
*/

/*
 * "turnRight" is a more user-friendly version of "turn" where the degree put into the function is
 * more positive if turned clockwise
 *
 * @requires degree >=0
 * @effects the robot is turned to the specified angle to the right
 * @modifies none
 */
void turnRight(int16_t degree){
	turnWithShaft(-degree);
}

/*
 * "turnLeft" is a more user-friendly version of "turn" where the degree put into the function is
 * more positive if turned counter-clockwise
 * 
 * It works exactly the same as "turn"
 *
 * @requires degree >=0
 * @effects the robot is turned to the specified angle to the right
 * @modifies none
 */

void turnLeft(int16_t degree){
	turnWithShaft(degree);
}

/**
 * turnToOrient turns the robot to the given orientation
 *
 * the currentSide will be updated to the default value, meaning that it
 * is the expected value for normal situation. The currentSide of value 4
 * will be manually adjusted in accident-handler functions.
 * 
 * @requires the robot is not obstructed in turning
 * @effects turns the robot to the given orientation
 * @modifies currentOrient, currentSide
 */
void turnToOrient(int8_t orientNum){
	if (currentOrient == orientNum)
		{}
	else if ((currentOrient == orientNum - 1) || (currentOrient == orientNum + 3))
		turnRight(90);
	else if ((currentOrient == orientNum - 2) || (currentOrient == orientNum + 2))
		turnRight(180);
	else if ((currentOrient == orientNum -3) || (currentOrient == orientNum + 1))
		turnLeft(90);
	else {
		printf("\nOrient Error");
		pause(1000);
	}
	currentSide = currentOrient;
	currentOrient = orientNum;
}

/*
 * "calibrateGyro" lets the robot rest for a brief period of time and reset its angle back to zero
 *
 * @requires none
 * @effects the angle measured by gyroscope is reset to zero
 * @modifies the angle stored in gyroscope
 */
void calibrateGyro(void){
	printf_P (PSTR("\nStabilizing..."));
	pause(1500);
	printf_P (PSTR("\nCalibrating     offset...\n"));
	gyro_init (GYRO_PORT, LSB_MS_PER_DEG, GYRO_CALI_TIME);
}

/**
 * "getCurrentAngle" returns the angle that the robot has turned during the turn
 * 
 * @requires none
 * @effects returns the angle that the robot has turned during the turn
 * @modifies none
 */
int16_t getCurrentAngle(void){
	return gyro_get_degrees();
}
